#include "Matrix3.h"
#include "Matrix4.h"
#include "Angles.h"
#include "Rotation.h"
#include "Quaternions.h"
#include "Math.h"
#include <string.h>

namespace Core
{
	rtMatrix3 rtMatrix3::mat3_zero(rtVector3(0, 0, 0), rtVector3(0, 0, 0), rtVector3(0, 0, 0));
	rtMatrix3 rtMatrix3::mat3_identity(rtVector3(1, 0, 0), rtVector3(0, 1, 0), rtVector3(0, 0, 1));

	inline rtMatrix3::rtMatrix3(void) 
	{
	}

	inline rtMatrix3::rtMatrix3(const rtVector3& x, const rtVector3& y, const rtVector3& z) 
	{
		m_Mat[0].x = x.x; m_Mat[0].y = x.y; m_Mat[0].z = x.z;
		m_Mat[1].x = y.x; m_Mat[1].y = y.y; m_Mat[1].z = y.z;
		m_Mat[2].x = z.x; m_Mat[2].y = z.y; m_Mat[2].z = z.z;
	}

	inline rtMatrix3::rtMatrix3(const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz) 
	{
		m_Mat[0].x = xx; m_Mat[0].y = xy; m_Mat[0].z = xz;
		m_Mat[1].x = yx; m_Mat[1].y = yy; m_Mat[1].z = yz;
		m_Mat[2].x = zx; m_Mat[2].y = zy; m_Mat[2].z = zz;
	}

	inline rtMatrix3::rtMatrix3(const float src[3][3]) 
	{
		memcpy(m_Mat, src, 3 * 3 * sizeof(float));
	}

	inline const rtVector3 &rtMatrix3::operator [] (int index) const 
	{

		return m_Mat[index];
	}

	inline rtVector3& rtMatrix3::operator [] (int index) 
	{

		return m_Mat[index];
	}

	inline rtMatrix3 rtMatrix3::operator - () const 
	{
		return rtMatrix3(-m_Mat[0][0], -m_Mat[0][1], -m_Mat[0][2],
			-m_Mat[1][0], -m_Mat[1][1], -m_Mat[1][2],
			-m_Mat[2][0], -m_Mat[2][1], -m_Mat[2][2]);
	}

	inline rtVector3 rtMatrix3::operator * (const rtVector3& vec) const 
	{
		return rtVector3(
			m_Mat[0].x * vec.x + m_Mat[1].x * vec.y + m_Mat[2].x * vec.z,
			m_Mat[0].y * vec.x + m_Mat[1].y * vec.y + m_Mat[2].y * vec.z,
			m_Mat[0].z * vec.x + m_Mat[1].z * vec.y + m_Mat[2].z * vec.z);
	}

	inline rtMatrix3 rtMatrix3::operator * (const rtMatrix3& a) const 
	{
		int i, j;
		const float *m1Ptr, *m2Ptr;
		float *dstPtr;
		rtMatrix3 dst;

		m1Ptr = reinterpret_cast<const float*>(this);
		m2Ptr = reinterpret_cast<const float*>(&a);
		dstPtr = reinterpret_cast<float*>(&dst);

		for (i = 0; i < 3; i++) 
		{
			for (j = 0; j < 3; j++) 
			{
				*dstPtr = m1Ptr[0] * m2Ptr[0 * 3 + j]
				+ m1Ptr[1] * m2Ptr[1 * 3 + j]
				+ m1Ptr[2] * m2Ptr[2 * 3 + j];
				dstPtr++;
			}
			m1Ptr += 3;
		}
		return dst;
	}

	inline rtMatrix3 rtMatrix3::operator * (const float a) const 
	{
		return rtMatrix3(
			m_Mat[0].x * a, m_Mat[0].y * a, m_Mat[0].z * a,
			m_Mat[1].x * a, m_Mat[1].y * a, m_Mat[1].z * a,
			m_Mat[2].x * a, m_Mat[2].y * a, m_Mat[2].z * a);
	}

	inline rtMatrix3 rtMatrix3::operator + (const rtMatrix3& a) const 
	{
		return rtMatrix3(
			m_Mat[0].x + a[0].x, m_Mat[0].y + a[0].y, m_Mat[0].z + a[0].z,
			m_Mat[1].x + a[1].x, m_Mat[1].y + a[1].y, m_Mat[1].z + a[1].z,
			m_Mat[2].x + a[2].x, m_Mat[2].y + a[2].y, m_Mat[2].z + a[2].z);
	}

	inline rtMatrix3 rtMatrix3::operator - (const rtMatrix3& a) const 
	{
		return rtMatrix3(
			m_Mat[0].x - a[0].x, m_Mat[0].y - a[0].y, m_Mat[0].z - a[0].z,
			m_Mat[1].x - a[1].x, m_Mat[1].y - a[1].y, m_Mat[1].z - a[1].z,
			m_Mat[2].x - a[2].x, m_Mat[2].y - a[2].y, m_Mat[2].z - a[2].z);
	}

	inline rtMatrix3& rtMatrix3::operator *= (const float a) 
	{
		m_Mat[0].x *= a; m_Mat[0].y *= a; m_Mat[0].z *= a;
		m_Mat[1].x *= a; m_Mat[1].y *= a; m_Mat[1].z *= a; 
		m_Mat[2].x *= a; m_Mat[2].y *= a; m_Mat[2].z *= a;

		return *this;
	}

	inline rtMatrix3& rtMatrix3::operator *= (const rtMatrix3& a) 
	{
		int i, j;
		const float *m2Ptr;
		float *m1Ptr, dst[3];

		m1Ptr = reinterpret_cast<float *>(this);
		m2Ptr = reinterpret_cast<const float *>(&a);

		for (i = 0; i < 3; i++) 
		{
			for (j = 0; j < 3; j++) 
			{
				dst[j]  = m1Ptr[0] * m2Ptr[0 * 3 + j]
				+ m1Ptr[1] * m2Ptr[1 * 3 + j]
				+ m1Ptr[2] * m2Ptr[2 * 3 + j];
			}
			m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
			m1Ptr += 3;
		}
		return *this;
	}

	inline rtMatrix3& rtMatrix3::operator += (const rtMatrix3& a) 
	{
		m_Mat[0].x += a[0].x; m_Mat[0].y += a[0].y; m_Mat[0].z += a[0].z;
		m_Mat[1].x += a[1].x; m_Mat[1].y += a[1].y; m_Mat[1].z += a[1].z;
		m_Mat[2].x += a[2].x; m_Mat[2].y += a[2].y; m_Mat[2].z += a[2].z;

		return *this;
	}

	inline rtMatrix3& rtMatrix3::operator -= (const rtMatrix3& a) 
	{
		m_Mat[0].x -= a[0].x; m_Mat[0].y -= a[0].y; m_Mat[0].z -= a[0].z;
		m_Mat[1].x -= a[1].x; m_Mat[1].y -= a[1].y; m_Mat[1].z -= a[1].z;
		m_Mat[2].x -= a[2].x; m_Mat[2].y -= a[2].y; m_Mat[2].z -= a[2].z;

		return *this;
	}

	inline rtVector3 operator * (const rtVector3& vec, const rtMatrix3& mat3) 
	{
		return mat3 * vec;
	}

	inline rtMatrix3 operator * (const float a, const rtMatrix3 &mat3) 
	{
		return mat3 * a;
	}

	inline rtVector3& operator*=(rtVector3& vec, const rtMatrix3& mat3) 
	{
		float x = mat3[0].x * vec.x + mat3[1].x * vec.y + mat3[2].x * vec.z;
		float y = mat3[0].y * vec.x + mat3[1].y * vec.y + mat3[2].y * vec.z;
		vec.z = mat3[0].z * vec.x + mat3[1].z * vec.y + mat3[2].z * vec.z;
		vec.x = x;
		vec.y = y;
		return vec;
	}

	inline bool rtMatrix3::compare(const rtMatrix3& a) const 
	{
		if (m_Mat[0].compare(a[0]) &&
			m_Mat[1].compare(a[1]) &&
			m_Mat[2].compare(a[2])) 
		{
			return true;
		}
		return false;
	}

	inline bool rtMatrix3::compare(const rtMatrix3& a, const float epsilon) const 
	{
		if (m_Mat[0].compare(a[0], epsilon) &&
			m_Mat[1].compare(a[1], epsilon) &&
			m_Mat[2].compare(a[2], epsilon)) 
		{
			return true;
		}
		return false;
	}

	inline bool rtMatrix3::operator == (const rtMatrix3& a) const 
	{
		return compare(a);
	}

	inline bool rtMatrix3::operator != (const rtMatrix3& a) const 
	{
		return !compare(a);
	}

	inline void rtMatrix3::zero(void) 
	{
		memset(m_Mat, 0, sizeof(rtMatrix3));
	}

	inline void rtMatrix3::identity(void) 
	{
		*this = mat3_identity;
	}

	inline bool rtMatrix3::isIdentity(const float epsilon) const 
	{
		return compare(mat3_identity, epsilon);
	}

	inline bool rtMatrix3::isSymmetric(const float epsilon) const 
	{
		if (rtMath::Fabs(m_Mat[0][1] - m_Mat[1][0]) > epsilon) 
		{
			return false;
		}
		if (rtMath::Fabs(m_Mat[0][2] - m_Mat[2][0]) > epsilon) 
		{
			return false;
		}
		if (rtMath::Fabs(m_Mat[1][2] - m_Mat[2][1]) > epsilon) 
		{
			return false;
		}
		return true;
	}

	inline bool rtMatrix3::isDiagonal(const float epsilon) const 
	{
		if (rtMath::Fabs(m_Mat[0][1]) > epsilon ||
			rtMath::Fabs(m_Mat[0][2]) > epsilon ||
			rtMath::Fabs(m_Mat[1][0]) > epsilon ||
			rtMath::Fabs(m_Mat[1][2]) > epsilon ||
			rtMath::Fabs(m_Mat[2][0]) > epsilon ||
			rtMath::Fabs(m_Mat[2][1]) > epsilon) 
		{
			return false;
		}
		return true;
	}

	inline bool rtMatrix3::isRotated(void) const 
	{
		return !compare(mat3_identity);
	}

	inline void rtMatrix3::projectVector(const rtVector3& src, rtVector3& dst) const 
	{
		dst.x = src * m_Mat[0];
		dst.y = src * m_Mat[1];
		dst.z = src * m_Mat[2];
	}

	inline void rtMatrix3::unprojectVector(const rtVector3& src, rtVector3& dst) const 
	{
		dst = m_Mat[0] * src.x + m_Mat[1] * src.y + m_Mat[2] * src.z;
	}

	inline bool rtMatrix3::fixDegeneracies(void) 
	{
		bool r = m_Mat[0].fixDegenerateNormal();
		r |= m_Mat[1].fixDegenerateNormal();
		r |= m_Mat[2].fixDegenerateNormal();
		return r;
	}

	inline bool rtMatrix3::fixDenormals(void) 
	{
		bool r = m_Mat[0].fixDenormals();
		r |= m_Mat[1].fixDenormals();
		r |= m_Mat[2].fixDenormals();
		return r;
	}

	inline float rtMatrix3::trace(void) const 
	{
		return (m_Mat[0][0] + m_Mat[1][1] + m_Mat[2][2]);
	}

	inline rtMatrix3 rtMatrix3::orthoNormalize(void) const 
	{
		rtMatrix3 ortho;

		ortho = *this;
		ortho[0].normalize();
		ortho[2].cross(m_Mat[0], m_Mat[1]);
		ortho[2].normalize();
		ortho[1].cross(m_Mat[2], m_Mat[0]);
		ortho[1].normalize();
		return ortho;
	}

	inline rtMatrix3 &rtMatrix3::orthoNormalizeSelf(void) 
	{
		m_Mat[0].normalize();
		m_Mat[2].cross(m_Mat[0], m_Mat[1]);
		m_Mat[2].normalize();
		m_Mat[1].cross(m_Mat[2], m_Mat[0]);
		m_Mat[1].normalize();
		return *this;
	}

	inline rtMatrix3 rtMatrix3::transpose(void) const 
	{
		return rtMatrix3(m_Mat[0][0], m_Mat[1][0], m_Mat[2][0],
			m_Mat[0][1], m_Mat[1][1], m_Mat[2][1],
			m_Mat[0][2], m_Mat[1][2], m_Mat[2][2]);
	}

	inline rtMatrix3 &rtMatrix3::transposeSelf(void) 
	{
		float tmp0, tmp1, tmp2;

		tmp0 = m_Mat[0][1];
		m_Mat[0][1] = m_Mat[1][0];
		m_Mat[1][0] = tmp0;
		tmp1 = m_Mat[0][2];
		m_Mat[0][2] = m_Mat[2][0];
		m_Mat[2][0] = tmp1;
		tmp2 = m_Mat[1][2];
		m_Mat[1][2] = m_Mat[2][1];
		m_Mat[2][1] = tmp2;

		return *this;
	}

	inline rtMatrix3 rtMatrix3::inverse(void) const 
	{
		rtMatrix3 invMat;

		invMat = *this;
		int r = invMat.inverseSelf();
		assert(r);
		return invMat;
	}

	inline rtMatrix3 rtMatrix3::inverseFast(void) const 
	{
		rtMatrix3 invMat;

		invMat = *this;
		int r = invMat.inverseFastSelf();
		assert(r);
		return invMat;
	}

	inline rtMatrix3 rtMatrix3::transposeMultiply(const rtMatrix3& b) const 
	{
		return rtMatrix3(m_Mat[0].x * b[0].x + m_Mat[1].x * b[1].x + m_Mat[2].x * b[2].x,
			m_Mat[0].x * b[0].y + m_Mat[1].x * b[1].y + m_Mat[2].x * b[2].y,
			m_Mat[0].x * b[0].z + m_Mat[1].x * b[1].z + m_Mat[2].x * b[2].z,
			m_Mat[0].y * b[0].x + m_Mat[1].y * b[1].x + m_Mat[2].y * b[2].x,
			m_Mat[0].y * b[0].y + m_Mat[1].y * b[1].y + m_Mat[2].y * b[2].y,
			m_Mat[0].y * b[0].z + m_Mat[1].y * b[1].z + m_Mat[2].y * b[2].z,
			m_Mat[0].z * b[0].x + m_Mat[1].z * b[1].x + m_Mat[2].z * b[2].x,
			m_Mat[0].z * b[0].y + m_Mat[1].z * b[1].y + m_Mat[2].z * b[2].y,
			m_Mat[0].z * b[0].z + m_Mat[1].z * b[1].z + m_Mat[2].z * b[2].z);
	}

	inline void transposeMultiply(const rtMatrix3& transpose, const rtMatrix3& b, rtMatrix3& dst) 
	{
		dst[0].x = transpose[0].x * b[0].x + transpose[1].x * b[1].x + transpose[2].x * b[2].x;
		dst[0].y = transpose[0].x * b[0].y + transpose[1].x * b[1].y + transpose[2].x * b[2].y;
		dst[0].z = transpose[0].x * b[0].z + transpose[1].x * b[1].z + transpose[2].x * b[2].z;
		dst[1].x = transpose[0].y * b[0].x + transpose[1].y * b[1].x + transpose[2].y * b[2].x;
		dst[1].y = transpose[0].y * b[0].y + transpose[1].y * b[1].y + transpose[2].y * b[2].y;
		dst[1].z = transpose[0].y * b[0].z + transpose[1].y * b[1].z + transpose[2].y * b[2].z;
		dst[2].x = transpose[0].z * b[0].x + transpose[1].z * b[1].x + transpose[2].z * b[2].x;
		dst[2].y = transpose[0].z * b[0].y + transpose[1].z * b[1].y + transpose[2].z * b[2].y;
		dst[2].z = transpose[0].z * b[0].z + transpose[1].z * b[1].z + transpose[2].z * b[2].z;
	}

	inline rtMatrix3 skewSymmetric(rtVector3 const& src) 
	{
		return rtMatrix3(0.0f, -src.z,  src.y, src.z,   0.0f, -src.x, -src.y,  src.x,   0.0f);
	}


	inline int rtMatrix3::getDimension(void) const 
	{
		return 9;
	}

	inline const float *rtMatrix3::toFloatPtr(void) const 
	{
		return m_Mat[0].toFloatPtr();
	}

	inline float *rtMatrix3::toFloatPtr(void) 
	{
		return m_Mat[0].toFloatPtr();
	}

	rtAngles rtMatrix3::toAngles(void) const 
	{
		rtAngles	    angles;
		double		theta;
		double		cp;
		float		sp;

		sp = m_Mat[0][2];

		if (sp > 1.0f) 
		{
			sp = 1.0f;
		} 
		else if (sp < -1.0f) 
		{
			sp = -1.0f;
		}

		theta = -asin(sp);
		cp = cos(theta);

		if (cp > 8192.0f * rtMath::FLT_EPSILON) 
		{
			angles.pitch	= RAD2DEG(theta);
			angles.yaw		= RAD2DEG(atan2(m_Mat[0][1], m_Mat[0][0]));
			angles.roll		= RAD2DEG(atan2(m_Mat[1][2], m_Mat[2][2]));
		} 
		else 
		{
			angles.pitch	= RAD2DEG(theta);
			angles.yaw		= RAD2DEG(-atan2(m_Mat[1][0], m_Mat[1][1]));
			angles.roll		= 0;
		}
		return angles;
	}


	rtQuaternions rtMatrix3::toQuat(void) const 
	{
		rtQuaternions		q;
		float		trace;
		float		s;
		float		t;
		int     	i;
		int			j;
		int			k;

		static int 	next[3] = {1, 2, 0};

		trace = m_Mat[0][0] + m_Mat[1][1] + m_Mat[2][2];

		if (trace > 0.0f) 
		{

			t = trace + 1.0f;
			s = rtMath::InvSqrt(t) * 0.5f;

			q[3] = s * t;
			q[0] = (m_Mat[2][1] - m_Mat[1][2]) * s;
			q[1] = (m_Mat[0][2] - m_Mat[2][0]) * s;
			q[2] = (m_Mat[1][0] - m_Mat[0][1]) * s;

		} 
		else 
		{

			i = 0;
			if (m_Mat[1][1] > m_Mat[0][0]) 
			{
				i = 1;
			}
			if (m_Mat[2][2] > m_Mat[i][i]) 
			{
				i = 2;
			}
			j = next[i];
			k = next[j];

			t = (m_Mat[i][i] - (m_Mat[j][j] + m_Mat[k][k])) + 1.0f;
			s = rtMath::InvSqrt(t) * 0.5f;

			q[i] = s * t;
			q[3] = (m_Mat[k][j] - m_Mat[j][k]) * s;
			q[j] = (m_Mat[j][i] + m_Mat[i][j]) * s;
			q[k] = (m_Mat[k][i] + m_Mat[i][k]) * s;
		}
		return q;
	}



	rtRotation rtMatrix3::toRotation(void) const 
	{
		rtRotation	r;
		float		trace;
		float		s;
		float		t;
		int     	i;
		int			j;
		int			k;
		static int 	next[3] = {1, 2, 0};

		trace = m_Mat[0][0] + m_Mat[1][1] + m_Mat[2][2];
		if (trace > 0.0f) 
		{

			t = trace + 1.0f;
			s = rtMath::InvSqrt(t) * 0.5f;

			r.m_Angle = s * t;
			r.m_Vec[0] = (m_Mat[2][1] - m_Mat[1][2]) * s;
			r.m_Vec[1] = (m_Mat[0][2] - m_Mat[2][0]) * s;
			r.m_Vec[2] = (m_Mat[1][0] - m_Mat[0][1]) * s;

		} 
		else 
		{

			i = 0;
			if (m_Mat[1][1] > m_Mat[0][0]) 
			{
				i = 1;
			}
			if (m_Mat[2][2] > m_Mat[i][i]) 
			{
				i = 2;
			}
			j = next[i];  
			k = next[j];

			t = (m_Mat[i][i] - (m_Mat[j][j] + m_Mat[k][k])) + 1.0f;
			s = rtMath::InvSqrt(t) * 0.5f;

			r.m_Vec[i]	= s * t;
			r.m_Angle		= (m_Mat[k][j] - m_Mat[j][k]) * s;
			r.m_Vec[j]	= (m_Mat[j][i] + m_Mat[i][j]) * s;
			r.m_Vec[k]	= (m_Mat[k][i] + m_Mat[i][k]) * s;
		}
		r.m_Angle = rtMath::ACos(r.m_Angle);
		if (rtMath::Fabs(r.m_Angle) < 1e-10f) 
		{
			r.m_Vec.set(0.0f, 0.0f, 1.0f);
			r.m_Angle = 0.0f;
		} 
		else 
		{
			r.m_Vec.normalize();
			r.m_Vec.fixDegenerateNormal();
			r.m_Angle *= 2.0f * rtMath::M_RAD2DEG;
		}

		r.m_Origin.zero();
		r.m_Axis = *this;
		r.m_AxisValid = true;
		return r;
	}

	inline rtMatrix4 rtMatrix3::toMat4(void) const 
	{
		return rtMatrix4(m_Mat[0][0], m_Mat[1][0], m_Mat[2][0], 0.0f,
			             m_Mat[0][1], m_Mat[1][1], m_Mat[2][1], 0.0f,
			             m_Mat[0][2], m_Mat[1][2], m_Mat[2][2], 0.0f,
			             0.0f,        0.0f,        0.0f,        1.0f);
	}

	rtVector3 rtMatrix3::toAngularVelocity(void) const 
	{
		rtRotation rotation = toRotation();
		return rotation.getVec() * DEG2RAD(rotation.getAngle());
	}

	float rtMatrix3::determinant(void) const 
	{

		float det2_12_01 = m_Mat[1][0] * m_Mat[2][1] - m_Mat[1][1] * m_Mat[2][0];
		float det2_12_02 = m_Mat[1][0] * m_Mat[2][2] - m_Mat[1][2] * m_Mat[2][0];
		float det2_12_12 = m_Mat[1][1] * m_Mat[2][2] - m_Mat[1][2] * m_Mat[2][1];

		return m_Mat[0][0] * det2_12_12 - m_Mat[0][1] * det2_12_02 + m_Mat[0][2] * det2_12_01;
	}


	bool rtMatrix3::inverseSelf(void) 
	{
		rtMatrix3 inverse;
		double det, invDet;

		inverse[0][0] = m_Mat[1][1] * m_Mat[2][2] - m_Mat[1][2] * m_Mat[2][1];
		inverse[1][0] = m_Mat[1][2] * m_Mat[2][0] - m_Mat[1][0] * m_Mat[2][2];
		inverse[2][0] = m_Mat[1][0] * m_Mat[2][1] - m_Mat[1][1] * m_Mat[2][0];

		det = m_Mat[0][0] * inverse[0][0] + m_Mat[0][1] * inverse[1][0] + m_Mat[0][2] * inverse[2][0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		inverse[0][1] = m_Mat[0][2] * m_Mat[2][1] - m_Mat[0][1] * m_Mat[2][2];
		inverse[0][2] = m_Mat[0][1] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][1];
		inverse[1][1] = m_Mat[0][0] * m_Mat[2][2] - m_Mat[0][2] * m_Mat[2][0];
		inverse[1][2] = m_Mat[0][2] * m_Mat[1][0] - m_Mat[0][0] * m_Mat[1][2];
		inverse[2][1] = m_Mat[0][1] * m_Mat[2][0] - m_Mat[0][0] * m_Mat[2][1];
		inverse[2][2] = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];

		m_Mat[0][0] = inverse[0][0] * invDet;
		m_Mat[0][1] = inverse[0][1] * invDet;
		m_Mat[0][2] = inverse[0][2] * invDet;

		m_Mat[1][0] = inverse[1][0] * invDet;
		m_Mat[1][1] = inverse[1][1] * invDet;
		m_Mat[1][2] = inverse[1][2] * invDet;

		m_Mat[2][0] = inverse[2][0] * invDet;
		m_Mat[2][1] = inverse[2][1] * invDet;
		m_Mat[2][2] = inverse[2][2] * invDet;

		return true;
	}


	bool rtMatrix3::inverseFastSelf(void) 
	{
		rtMatrix3 inverse;
		double det, invDet;

		inverse[0][0] = m_Mat[1][1] * m_Mat[2][2] - m_Mat[1][2] * m_Mat[2][1];
		inverse[1][0] = m_Mat[1][2] * m_Mat[2][0] - m_Mat[1][0] * m_Mat[2][2];
		inverse[2][0] = m_Mat[1][0] * m_Mat[2][1] - m_Mat[1][1] * m_Mat[2][0];

		det = m_Mat[0][0] * inverse[0][0] + m_Mat[0][1] * inverse[1][0] + m_Mat[0][2] * inverse[2][0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		inverse[0][1] = m_Mat[0][2] * m_Mat[2][1] - m_Mat[0][1] * m_Mat[2][2];
		inverse[0][2] = m_Mat[0][1] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][1];
		inverse[1][1] = m_Mat[0][0] * m_Mat[2][2] - m_Mat[0][2] * m_Mat[2][0];
		inverse[1][2] = m_Mat[0][2] * m_Mat[1][0] - m_Mat[0][0] * m_Mat[1][2];
		inverse[2][1] = m_Mat[0][1] * m_Mat[2][0] - m_Mat[0][0] * m_Mat[2][1];
		inverse[2][2] = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];

		m_Mat[0][0] = inverse[0][0] * invDet;
		m_Mat[0][1] = inverse[0][1] * invDet;
		m_Mat[0][2] = inverse[0][2] * invDet;

		m_Mat[1][0] = inverse[1][0] * invDet;
		m_Mat[1][1] = inverse[1][1] * invDet;
		m_Mat[1][2] = inverse[1][2] * invDet;

		m_Mat[2][0] = inverse[2][0] * invDet;
		m_Mat[2][1] = inverse[2][1] * invDet;
		m_Mat[2][2] = inverse[2][2] * invDet;

		return true;
	}

	rtMatrix3 rtMatrix3::inertiaTranslate(const float mass, const rtVector3& centerOfMass, const rtVector3& translation) const 
	{
		rtMatrix3 m;
		rtVector3 newCenter;

		newCenter = centerOfMass + translation;

		m[0][0] = mass * ((centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2])
			- (newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2]));
		m[1][1] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2])
			- (newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2]));
		m[2][2] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1])
			- (newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1]));

		m[0][1] = m[1][0] = mass * (newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1]);
		m[1][2] = m[2][1] = mass * (newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2]);
		m[0][2] = m[2][0] = mass * (newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2]);

		return (*this) + m;
	}

	rtMatrix3& rtMatrix3::inertiaTranslateSelf(const float mass, const rtVector3& centerOfMass, const rtVector3& translation) 
	{
		rtMatrix3 m;
		rtVector3 newCenter;

		newCenter = centerOfMass + translation;

		m[0][0] = mass * ((centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2])
			- (newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2]));
		m[1][1] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2])
			- (newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2]));
		m[2][2] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1])
			- (newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1]));

		m[0][1] = m[1][0] = mass * (newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1]);
		m[1][2] = m[2][1] = mass * (newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2]);
		m[0][2] = m[2][0] = mass * (newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2]);

		(*this) += m;

		return (*this);
	}

	rtMatrix3 rtMatrix3::inertiaRotate(const rtMatrix3& rotation) const 
	{
		return rotation.transpose() * (*this) * rotation;
	}


	rtMatrix3 &rtMatrix3::inertiaRotateSelf(const rtMatrix3& rotation) 
	{
		*this = rotation.transpose() * (*this) * rotation;
		return *this;
	}


}
